clear all;
close all;
clc;

%% parsing out data from bob's eval
fs = 1000; %Hz

% filename = 'RF_mvc2';
% [labels_anc,time_anc,EMG_RFmax,rel_angle,step_number,forces] = readColDataANC(strcat(filename,'.anc'),23,11,1);
% EMG_RFmax = EMG_RFmax(:,1);
% EMG_RFmax = highpass(EMG_RFmax,fs,100);
% EMG_RFmax = bandstop(EMG_RFmax,fs,59,61);
% EMG_RFmax = lowpass(EMG_RFmax,fs,50);
% EMG_RFmax = abs(EMG_RFmax-mean(EMG_RFmax));
% max_RF = max(EMG_RFmax);
% 
% filename = 'VL_mvc1';
% [labels_anc,time_anc,EMG_VLmax,rel_angle,step_number,forces] = readColDataANC(strcat(filename,'.anc'),23,11,1);
% EMG_VLmax = EMG_VLmax(:,2);
% EMG_VLmax = highpass(EMG_VLmax,fs,100);
% EMG_VLmax = bandstop(EMG_VLmax,fs,59,61);
% EMG_VLmax = lowpass(EMG_VLmax,fs,50);
% EMG_VLmax = abs(EMG_VLmax-mean(EMG_VLmax));
% max_VL = max(EMG_VLmax);
% 
% filename = 'MH_mvc2';
% [labels_anc,time_anc,EMG_MHmax,rel_angle,step_number,forces] = readColDataANC(strcat(filename,'.anc'),23,11,1);
% EMG_MHmax = EMG_MHmax(:,3);
% EMG_MHmax = highpass(EMG_MHmax,fs,100);
% EMG_MHmax = bandstop(EMG_MHmax,fs,59,61);
% EMG_MHmax = lowpass(EMG_MHmax,fs,50);
% EMG_MHmax = abs(EMG_MHmax-mean(EMG_MHmax));
% max_MH = max(EMG_MHmax);
% 
% filename = 'AL_mvc1';
% [labels_anc,time_anc,EMG_ALmax,rel_angle,step_number,forces] = readColDataANC(strcat(filename,'.anc'),23,11,1);
% EMG_ALmax = EMG_ALmax(:,4);
% EMG_ALmax = highpass(EMG_ALmax,fs,100);
% EMG_ALmax = bandstop(EMG_ALmax,fs,59,61);
% EMG_ALmax = lowpass(EMG_ALmax,fs,50);
% EMG_ALmax = abs(EMG_ALmax-mean(EMG_ALmax));
% max_AL = max(EMG_ALmax);

filename = 'eval1';
[labels_anc,time_anc,EMG,rel_angle,step_number,forces] = readColDataANC(strcat(filename,'.anc'),23,11,1);
% EMG = highpass(EMG,fs,100);
EMG = bandstop(EMG,fs,59,61);
EMG = lowpass(EMG,fs,50);
EMG = [abs(EMG(:,1)-mean(EMG(:,1))), ...
    abs(EMG(:,2)-mean(EMG(:,2))), ...
    abs(EMG(:,3)-mean(EMG(:,3))), ...
    abs(EMG(:,4)-mean(EMG(:,4)))];
torque = -rel_angle/204.8/.75*20*.308;

%% Loading kinematic data for step parsing
 % load .trc file
    [labels,time_trc,LASIS,RASIS,Sacrum,Sac_offset,LThigh_up,LThigh_front,LThigh_rear,...
        LShank_up, LShank_front, LShank_rear, LAnkle_lat,  LHeel, LToe, RThigh_up, RThigh_front,RThigh_rear,...  %switched RAnkle_lat and LAnkle_lat for no-orthosis_2km1
        RShank_up, RShank_front, RShank_rear, RAnkle_lat,RHeel, RToe]...
        = readColDataMA_dyn(strcat(filename,'.trc'),68,6,1);

    
    %constants
    MA_filter = 8; %Hz
    interplength = 2000;
    torque_trials = [2 5 8 12 14 17 22 24 26 28 31 33];
    GC = linspace(0,100,interplength);
    
    %% analyzing data
    dt_trc = time_trc(2)-time_trc(1);
     dt_anc = time_anc(2)-time_anc(1);
 xdata = [LASIS(:,1),RASIS(:,1),Sacrum(:,1),LThigh_up(:,1),LThigh_front(:,1),LThigh_rear(:,1),...
        LShank_up(:,1), LShank_front(:,1), LShank_rear(:,1), LAnkle_lat(:,1), LHeel(:,1),  LToe(:,1), RThigh_up(:,1),RThigh_front(:,1),RThigh_rear(:,1),...
        RShank_up(:,1), RShank_front(:,1), RShank_rear(:,1), RAnkle_lat(:,1), RHeel(:,1),  RToe(:,1), Sac_offset(:,1)]/1000;

    ydata = [LASIS(:,2),RASIS(:,2),Sacrum(:,2),LThigh_up(:,2),LThigh_front(:,2),LThigh_rear(:,2),...
        LShank_up(:,2), LShank_front(:,2), LShank_rear(:,2), LAnkle_lat(:,2), LHeel(:,2), LToe(:,2), RThigh_up(:,2),RThigh_front(:,2),RThigh_rear(:,2),...
        RShank_up(:,2), RShank_front(:,2), RShank_rear(:,2), RAnkle_lat(:,2),RHeel(:,2), RToe(:,2), Sac_offset(:,2)]/1000;

    zdata = [LASIS(:,3),RASIS(:,3),Sacrum(:,3),LThigh_up(:,3),LThigh_front(:,3),LThigh_rear(:,3),...
        LShank_up(:,3), LShank_front(:,3), LShank_rear(:,3), LAnkle_lat(:,3), LHeel(:,3), LToe(:,3), RThigh_up(:,3),RThigh_front(:,3),RThigh_rear(:,3),...
        RShank_up(:,3), RShank_front(:,3), RShank_rear(:,3), RAnkle_lat(:,3), RHeel(:,3), RToe(:,3), Sac_offset(:,3)]/1000;
    [b,a] = butter(5,MA_filter/50); %100 Hz sampling rate
    xdata = filtfilt(b,a,xdata); ydata = filtfilt(b,a,ydata); zdata = filtfilt(b,a,zdata);

    %% Finding Heel Strike/Toe off
    [LHS,LHS_locs] = findpeaks(xdata(:,12)); 
    
%%  Parsing out steps

for i = 2:length(LHS_locs)
        percent_s_EMG = (time_anc(10*LHS_locs(i-1):10*LHS_locs(i))-10*time_anc(LHS_locs(i-1)))...
            /(time_anc(10*LHS_locs(i))-time_anc(10*LHS_locs(i-1)));
    RF_s = EMG(10*LHS_locs(i-1):10*LHS_locs(i),1); RF(i,:) = interp1(percent_s_EMG,RF_s,linspace(0,1,interplength));
    VL_s = EMG(10*LHS_locs(i-1):10*LHS_locs(i),2); VL(i,:) = interp1(percent_s_EMG,VL_s,linspace(0,1,interplength));
    MH_s = EMG(10*LHS_locs(i-1):10*LHS_locs(i),3); MH(i,:) = interp1(percent_s_EMG,MH_s,linspace(0,1,interplength));
    AL_s = EMG(10*LHS_locs(i-1):10*LHS_locs(i),4); AL(i,:) = interp1(percent_s_EMG,AL_s,linspace(0,1,interplength));
    torque_s = torque(10*LHS_locs(i-1):10*LHS_locs(i)); torque_l(i,:) = interp1(percent_s_EMG,torque_s,linspace(0,1,interplength));
end
m=0;
n=0;
for i = 1:max(torque_trials)
    if any(i == torque_trials)
        m = m+1;
        m_RF_T(m,:) = RF(i,:);
       m_VL_T(m,:) = VL(i,:);
       m_MH_T(m,:) = MH(i,:);
       m_AL_T(m,:) = AL(i,:);
       torque_T(m,:) = torque_l(i,:);
    else
        n = n+1;
        m_RF(n,:) = RF(i,:);
        m_VL(n,:) = VL(i,:);
        m_MH(n,:) = MH(i,:);
        m_AL(n,:) = AL(i,:);   
    end
end

mean_RF_T = mean(m_RF_T);
mean_VL_T = mean(m_VL_T);
mean_MH_T = mean(m_MH_T);
mean_AL_T = mean(m_AL_T);
mean_torque = mean(torque_T);
ind = find(mean_torque>2);

mean_RF = mean(m_RF);
mean_VL = mean(m_VL);
mean_MH = mean(m_MH);
mean_AL = mean(m_AL);


figure;


subplot(4,2,2);hold on;
plot(GC,mean_RF_T)
title('RF ')
y = get(gca,'Ylim');
plot(ind/20,y(2)-1,'LineWidth',3,'Color','k')
subplot(4,2,1);hold on;
plot(GC,mean_RF)
set(gca,'Ylim',y);
title('RF (no assistance)')

subplot(4,2,4);hold on;
plot(GC,mean_VL_T)
title('VL')
y = get(gca,'Ylim');
plot(ind/20,y(2)-1,'LineWidth',3,'Color','k')
subplot(4,2,3);hold on;
plot(GC,mean_VL)
set(gca,'Ylim',y);
title('VL (no assistance)')

subplot(4,2,6);hold on;
plot(GC,mean_MH_T)
y = get(gca,'Ylim');
plot(ind/20,y(2)-1,'LineWidth',3,'Color','k')
title('MH')
subplot(4,2,5);hold on;
plot(GC,mean_MH)
set(gca,'Ylim',y);
title('MH (no assistance)')

subplot(4,2,8);hold on;
plot(GC,mean_AL_T)
y = get(gca,'Ylim');
plot(ind/20,y(2)-1,'LineWidth',3,'Color','k')
title('AL')
subplot(4,2,7);hold on;
plot(GC,mean_AL)
set(gca,'Ylim',y);
title('AL (no assistance)')

set(gcf,'Position',[440    47   835   751])